/*
uwb.h
offer uwb location function realzy
designed by lunanting

----------------------
2020-03-31 14:10:06
create file
----------------------
*/

#include "uwb.h"
#include "string.h"
#include "cli_cmd.h"
#include "aos/kernel.h"
#include "stdlib.h"

static UWB_state uwb_state={0};

void xp_uwb_recive_data_thread(void *arg);

/*
uwb init
param:
    port:uart port
    speed:uart speed
return:
    0=success
    -1=init uart fail
*/
int xp_uwb_init(uint8_t port,int speed,u16 id){
    uart_dev_t *uart=&uwb_state.uart;
    int ret=0;

    uart->port=port;
    uart->config.baud_rate=speed;
    uart->config.data_width=DATA_WIDTH_8BIT;
    uart->config.parity=NO_PARITY;
    uart->config.stop_bits=STOP_BITS_1;
    uart->config.flow_control=FLOW_CONTROL_DISABLED;
    uart->config.mode=MODE_TX_RX;

    ret=hal_uart_init(uart);
    if(ret!=0){
        uwb_state.init=0;
        println("uwb init fail! !hal_uart_init() error code=%d",ret);
        return -1;
    }
    else{
        println("uwb init success! by 485 bus, uart port=%d",port);
    }
    uwb_state.id=id;
    uwb_state.init=1;
    aos_task_new("uwb_thread",xp_uwb_recive_data_thread,NULL,2100);
    return 0;
}



/*
analysis uwb data thread
*/
static void xp_uwb_recive_data_thread(void *arg){
    int ret=0;
    uint8_t rec_buf[33];
    uint32_t rec_len;
    int sum=0,x=0,y=0;
    u16 id;
    println("uwb recive data thread run!");
    if(uwb_state.init!=1){
        println("please init uwb module first !");
        aos_task_exit(0);
    }
    while(1){
        ret=hal_uart_recv_II(&uwb_state.uart,rec_buf,33,&rec_len,10);
        if(ret!=0){
            continue;
        }
        // print_buf(rec_buf,33,8,16);
        if(rec_buf[2]!=0x2a)continue;
        // sum=xp_check_sum(rec_buf+2,28);
        // if(sum!=rec_buf[30]){
        //     println("uwb checksum error!");
        //     continue;
        // }
        memcpy(&id,rec_buf+4,2);
        if(id!=uwb_state.id){println("uwb lable id error!");continue;}
		memcpy(&x,rec_buf+6,4);
		memcpy(&y,rec_buf+10,4);
		if(abs(x-uwb_state.data.x)>10000||abs(y-uwb_state.data.y)>10000)continue;
		uwb_state.data.x=x;
		uwb_state.data.y=y;
//        memcpy(&uwb_state.data.z,rec_buf+14,4);
        memcpy(&uwb_state.data.timestamp,rec_buf+18,8);
    }
}

/*
get uwb location
param:
    data:recive location data
return:
    0=succcess
    -1=uart init fail
    -2=checksum fail
    -3=uart uninit
*/
int xp_uwb_get_location(UWB_data *data){
    if(data==NULL){
        println("error:xp_uwb_get_location() input data ==NULL !");
        return -1;
    }
    memcpy(data,&uwb_state.data,sizeof(UWB_data));
    return 0;
}

/*
check sum
param:
    data:check data
    len:data length
return:check sum
*/
int8_t xp_check_sum(const uint8_t *data,int len){
    int sum=0;
	
    for(int i=0;i<len;i++){
        sum+=data[i];
    }
    if(sum>0xff){
        sum=~sum;
        sum+=1;
    }
    sum=sum&0xff;
    return (uint8_t)sum;
}



/***************************test function***********************/
/*
test uwb location data
*/
void xp_uwb_test(void){
    UWB_data location={0};

    if(xp_uwb_get_location(&location)==0){
        println("uwb location:	x=%d  y=%d",location.x,location.y);
    }
    else{
        println("get uwb location data fail!");
    }

}

/*
uwb debug
*/
int xp_uwb_debug(char *type,char *fun,char *param){

    if(strcmp(type,"uwb")!=0)return 0;
    if(strcmp(fun,"location")==0){
        println("uwb location:	x=%d  y=%d",uwb_state.data.x,uwb_state.data.y);
    }
    else
    {
        return 0;
    }
    
    return 1;
}


//end of the file
